#!/usr/bin/env python3
"""
System Monitor - 系统监控和诊断
实时监控系统状态和性能
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import psutil
import time
import json
import requests
from typing import Dict, List
import threading

class SystemMonitor(Node):
    def __init__(self):
        super().__init__('system_monitor')
        
        # 监控配置
        # 通用设备IP（避免嵌入特定设备名）
        self.device_ip = "192.168.2.42"
        self.echokit_server = "localhost:8060"  # EchoKit Server
        
        # 状态变量
        self.system_status = {
            'device_connected': False,
            'echokit_connected': False,
            'ros2_bridge_active': False,
            'voice_processor_active': False,
            'cpu_usage': 0.0,
            'memory_usage': 0.0,
            'network_latency': 0.0,
            'last_command_time': 0.0,
            'command_success_rate': 1.0
        }
        
        self.command_stats = {
            'total_commands': 0,
            'successful_commands': 0,
            'failed_commands': 0,
            'avg_response_time': 0.0
        }
        
        # 创建QoS配置（与voice_control_node兼容）
        self.cmd_vel_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,  # 与voice_control_node的发布器兼容
            durability=DurabilityPolicy.VOLATILE,
            depth=1
        )
        
        # 发布器
        self.status_pub = self.create_publisher(String, 'system_status', 10)
        self.diagnostics_pub = self.create_publisher(String, 'system_diagnostics', 10)
        
        # 订阅器（使用BEST_EFFORT QoS与voice_control_node兼容）
        self.cmd_vel_sub = self.create_subscription(
            Twist, 'cmd_vel', self.monitor_commands, self.cmd_vel_qos)
        
        # 定时器
        self.monitor_timer = self.create_timer(2.0, self.monitor_system)  # 每2秒监控
        self.diagnostics_timer = self.create_timer(10.0, self.run_diagnostics)  # 每10秒诊断
        
        self.get_logger().info("System Monitor initialized")

    def monitor_system(self):
        """监控系统状态"""
        # 更新系统资源使用情况
        self.system_status['cpu_usage'] = psutil.cpu_percent()
        self.system_status['memory_usage'] = psutil.virtual_memory().percent
        
        # 检查网络连接
        self.check_connections()
        
        # 发布状态
        self.publish_status()

    def check_connections(self):
        """检查各组件连接状态"""
        # 检查设备连接
        self.system_status['device_connected'] = self.ping_device()
        
        # 检查EchoKit Server连接
        self.system_status['echokit_connected'] = self.ping_echokit_server()
        
        # 检查ROS2节点状态
        self.check_ros2_nodes()

    def ping_device(self) -> bool:
        """检查目标设备网络连通性（仅检查网络可达性）"""
        try:
            import subprocess
            start_time = time.time()
            # 使用ping检查网络连通性，而不是HTTP
            result = subprocess.run(['ping', '-c', '2', self.device_ip],
                                    capture_output=True, timeout=5)
            self.system_status['network_latency'] = (time.time() - start_time) * 1000
            return result.returncode == 0
        except:
            self.system_status['network_latency'] = 9999
            return False

    def ping_echokit_server(self) -> bool:
        """检查EchoKit Server连接"""
        try:
            response = requests.get(f"http://{self.echokit_server}/health", timeout=2)
            return response.status_code == 200
        except:
            return False

    def check_ros2_nodes(self):
        """检查ROS2节点状态"""
        try:
            # 这里可以添加更复杂的节点检查逻辑
            # 暂时简化处理
            self.system_status['ros2_bridge_active'] = True
            self.system_status['voice_processor_active'] = True
        except:
            self.system_status['ros2_bridge_active'] = False
            self.system_status['voice_processor_active'] = False

    def monitor_commands(self, msg: Twist):
        """监控命令执行"""
        self.system_status['last_command_time'] = time.time()
        self.command_stats['total_commands'] += 1
        
        # 简单的成功率计算（这里假设所有命令都成功）
        self.command_stats['successful_commands'] += 1
        self.system_status['command_success_rate'] = (
            self.command_stats['successful_commands'] / 
            max(1, self.command_stats['total_commands'])
        )

    def run_diagnostics(self):
        """运行系统诊断"""
        diagnostics = {
            'timestamp': time.time(),
            'system_health': self.calculate_system_health(),
            'recommendations': self.generate_recommendations(),
            'performance_metrics': self.get_performance_metrics()
        }
        
        # 发布诊断信息
        msg = String()
        msg.data = json.dumps(diagnostics, indent=2)
        self.diagnostics_pub.publish(msg)
        
        # 记录关键问题
        if diagnostics['system_health'] < 0.7:
            self.get_logger().warn(f"System health low: {diagnostics['system_health']:.2f}")

    def calculate_system_health(self) -> float:
        """计算系统健康度"""
        health_score = 1.0
        
        # 连接状态权重
        if not self.system_status['device_connected']:
            health_score -= 0.4
        if not self.system_status['echokit_connected']:
            health_score -= 0.2
        if not self.system_status['ros2_bridge_active']:
            health_score -= 0.2
        
        # 性能权重
        if self.system_status['cpu_usage'] > 80:
            health_score -= 0.1
        if self.system_status['memory_usage'] > 80:
            health_score -= 0.1
        if self.system_status['network_latency'] > 1000:
            health_score -= 0.1
        
        return max(0.0, health_score)

    def generate_recommendations(self) -> List[str]:
        """生成优化建议"""
        recommendations = []
        
        if not self.system_status['device_connected']:
            recommendations.append("检查设备网络连接和电源状态")
        
        if not self.system_status['echokit_connected']:
            recommendations.append("检查EchoKit Server是否正常运行")
        
        if self.system_status['cpu_usage'] > 80:
            recommendations.append("CPU使用率过高，考虑优化程序或升级硬件")
        
        if self.system_status['memory_usage'] > 80:
            recommendations.append("内存使用率过高，考虑释放内存或增加RAM")
        
        if self.system_status['network_latency'] > 500:
            recommendations.append("网络延迟较高，检查网络连接质量")
        
        if self.system_status['command_success_rate'] < 0.9:
            recommendations.append("命令成功率较低，检查系统配置")
        
        return recommendations

    def get_performance_metrics(self) -> Dict:
        """获取性能指标"""
        return {
            'uptime': time.time() - self.get_clock().now().nanoseconds / 1e9,
            'total_commands': self.command_stats['total_commands'],
            'success_rate': self.system_status['command_success_rate'],
            'avg_cpu': self.system_status['cpu_usage'],
            'avg_memory': self.system_status['memory_usage'],
            'network_latency': self.system_status['network_latency']
        }

    def publish_status(self):
        """发布系统状态"""
        status_msg = String()
        status_msg.data = json.dumps(self.system_status, indent=2)
        self.status_pub.publish(status_msg)

    def get_system_report(self) -> str:
        """生成系统报告"""
        health = self.calculate_system_health()
        recommendations = self.generate_recommendations()
        
        report = f"""
    === 系统状态报告 ===
    系统健康度: {health:.1%}
    设备连接: {'✅' if self.system_status['device_connected'] else '❌'}
    EchoKit连接: {'✅' if self.system_status['echokit_connected'] else '❌'}
    ROS2桥接: {'✅' if self.system_status['ros2_bridge_active'] else '❌'}
    语音处理: {'✅' if self.system_status['voice_processor_active'] else '❌'}

    性能指标:
    - CPU使用率: {self.system_status['cpu_usage']:.1f}%
    - 内存使用率: {self.system_status['memory_usage']:.1f}%
    - 网络延迟: {self.system_status['network_latency']:.1f}ms
    - 命令成功率: {self.system_status['command_success_rate']:.1%}

    优化建议:
    {chr(10).join(f'- {rec}' for rec in recommendations) if recommendations else '- 系统运行正常'}
    """
        return report

def main(args=None):
    rclpy.init(args=args)
    monitor = SystemMonitor()
    
    try:
        rclpy.spin(monitor)
    except KeyboardInterrupt:
        pass
    finally:
        monitor.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()